classdef SdInertNavInt < matlab.System & matlab.system.mixin.Propagates ...
        & matlab.system.mixin.Nondirect & matlab.system.mixin.SampleTime
    % SdInertNavInt 捷联惯性导航数值积分算法
    %
    % NOTE: When renaming the class name SdInertNavInt, the file name
    % and constructor name must be updated to use the class name.
    %
    % This template includes most, but not all, possible properties, attributes,
    % and methods that you can implement for a System object in Simulink.
    %
    % 说明可参考sinistep函数 TODO: 增加类说明
    
    % NOTE: 下述函数可以有不同实现：rv2dcm_5thod, rv2quat_5thod, Dsalpha_l_2ndod,
    % Dsupsilon_l_2ndod, drScrl_l_2ndod, DrRot_m_exactform_5thod, calcearthpar_pgs
    
    % Public, tunable properties
    properties
        
    end
    
    % Public, non-tunable properties
    properties(Nontunable)
        Cfg_NSInLS (1, 1) double {mustBeInteger} = 1;
        Cfg_HSInNS (1, 1) double {mustBeInteger} = 1;
        Cfg_UseDCMInAttCalc (1, 1) logical = true;
        Cfg_UseNormOrthoInAttCalc (1, 1) logical = true;
        Cfg_UseHiResPosNSCal (1, 1) logical = true;
        Cfg_UseExtGravity (1, 1) logical = true;
        Cfg_NavCoordType (1, 1) NavCoordType = NavCoordType.GEO;
        
        Par_Tl (1, 1) double {mustBePositive} = 0.005
        Par_C (1, 3) double {mustBeReal} = zeros(1, 3)
        
        % 以下默认参数基于WGS84 TODO: 改为CGCS2000中的参数值及J2、J4
        Earth_RE (1, 1) double {mustBePositive} = 6378137
        Earth_f (1, 1) double {mustBeNonnegative} = 1/298.257223563
        Earth_mu (1, 1) double {mustBeNonnegative} = 3.986004418e+14
        Earth_J2 (1, 1) double {mustBeReal} = 1.082627e-3
        Earth_J3 (1, 1) double {mustBeReal}= -2.5327e-6
        Earth_omegaIE (1, 1) double {mustBeNonnegative} = 7.292115e-5
        
        InitCon_LLA_n (3, 1) double {mustBeReal} = [pi/6 110/180*pi 0]
        InitCon_LLA_n1 (3, 1) double {mustBeReal} = [pi/6 110/180*pi 0]
        InitCon_g (1, 1) double {mustBeNonnegative} = 9.8
        InitCon_vN_m (3, 1) double {mustBeReal} = zeros(3, 1)
        InitCon_vN_m1 (3, 1) double {mustBeReal} = zeros(3, 1)
        InitCon_vN_n1 (3, 1) double {mustBeReal} = zeros(3, 1)
        InitCon_CBL (3, 3) double {mustBeReal} = eye(3)
        InitCon_specVelInc_lx (3, :) double {mustBeReal} = zeros(3, 1)
        InitCon_angleInc_lx (3, :) double {mustBeReal} = zeros(3, 1)
    end
    
    properties(SetAccess = private, Nontunable)
        BCAU (1, 1) BodyCoordAttUpd
        BCSVU (1, 1) BodyCoordSpecVelUpd
    end
    
    properties(DiscreteState)
        DS_cycCnt
        DS_n
        DS_m
        DS_l
        DS_Dalpha_lx
        DS_Dupsilon_lx
        DS_supsilon_l
        DS_salpha_l
        DS_CB_mL_n
        DS_qB_mL_n
        DS_CB_mL_n1
        DS_qB_mL_n1
        DS_CB_m1L_n1
        DS_qB_m1L_n1
        DS_CL_n1L_m
        DS_CL_n1L_m1
        DS_vN_m
        DS_vN_m1
        DS_vN_m2
        DS_CNE_n
        DS_CNE_n1
        DS_h_n
        DS_h_n1
        DS_sumDRN_m
        DS_sumDRN_m1
        DS_eVC3_n
        DS_eVC3_n1
        DS_omegaIEN
        DS_g
        DS_beta_l
        DS_DvScul_l
        DS_DrScrl_l
        DS_delh_n
        DS_FCN_n
        DS_FCN_n1
        DS_FCN_n2
        DS_rhoNz_n
        DS_rhoNz_n1
        DS_rhoNz_n2
        DS_gN_n
        DS_gN_n1
        DS_gN_n2
        DS_omegaIEN_n
        DS_omegaIEN_n1
        DS_omegaIEN_n2
    end
    
    properties(Nontunable, Constant)
        Cst_CNL = [0 1 0; 1 0 0; 0 0 -1]
        Cst_uZNN = [0 0 1]'
        Cst = struct('CNL', [0 1 0; 1 0 0; 0 0 -1], 'uZNN', [0 0 1]');
        Cst_OutBusName = "SdInertNavIntOutBus";
        Cst_AuxOutBusName = "SdInertNavIntAuxOutBus";
    end
    
    % Pre-computed constants
    properties(Access = private)
        EarthCst
    end
    
    methods
        % Constructor
        function obj = SdInertNavInt(varargin)
            % Support name-value pair arguments when constructing object
            setProperties(obj, nargin, varargin{:})
            obj.BCAU = BodyCoordAttUpd;
            obj.BCSVU = BodyCoordSpecVelUpd;
        end
        
        function obj = setPos(obj, CNE)
            if ~any(any(isnan(CNE)))
                obj.DS_CNE_n = CNE;
                obj.calcEarthPar();
            end
        end
        
        function obj = setHeight(obj, h)
            if ~isnan(h)
                obj.DS_h_n = h;
                obj.calcEarthPar();
            end
        end
        
        function obj = setVel(obj, v)
            isValid = ~isnan(vN);
            obj.DS_vN_m(isValid) = v(isValid); % 允许单独设置速度分量元素
            if any(isValid)
                obj.calcEarthPar();
            end
        end
        
        function obj = setAttDCM(obj, CBL)
            if obj.Cfg_UseDCMInAttCalc
                if ~any(any(isnan(CBL)))
                    obj.DS_CB_mL_n = CBL;
                end
            end
        end
        
        function obj = setAttQuat(obj, qBL)
            if ~obj.Cfg_UseDCMInAttCalc
                if ~any(isnan(qBL), 2)
                    obj.DS_qB_mL_n = qBL;
                end
            end
        end
        
        function obj = setPrevSpecVelInc(obj, Dupsilon_lx)
            if ~any(isnan(Dupsilon_lx))
                obj.DS_Dupsilon_lx(:, 1) = Dupsilon_lx;
            end
        end
        
        function obj = setPrevAngleInc(obj, Dalpha_lx)
            if ~any(isnan(Dalpha_lx))
                obj.DS_Dalpha_lx(:, 1) = Dalpha_lx;
            end
        end
        
        function [out, auxOut] = stepThrough(obj, specVelInc, angleInc, g, hAltm, ...
                CNEExt, hExt, vNExt, CBLExt, qBLExt, beta_m_ext, DvScul_m_ext, DrScrl_m_ext)
            m = size(specVelInc, 1);
            if nargin < 13
                DrScrl_m_ext = NaN(m, 3);
            end
            if nargin < 12
                DvScul_m_ext = NaN(m, 3);
            end
            if nargin < 11
                beta_m_ext = NaN(m, 3);
            end
            if nargin < 10
                qBLExt = NaN(m, 4);
            end
            if nargin < 9
                CBLExt = NaN(3, 3, m);
            end
            if nargin < 8
                vNExt = NaN(m, 3);
            end
            if nargin < 7
                hExt = NaN(m, 1);
            end
            if nargin < 6
                CNEExt = NaN(3, 3, m);
            end
            if nargin < 5
                hAltm = NaN(m, 1);
            end
            if (nargin<4) || isempty(g)
                g = repmat(obj.InitCon_g, m, 1);
            end
            
            hWB = waitbar_step(0, 0.01, 'Running strapdown inertial navigation integration.');
            for i=1:m
                waitbar_step(i/m, 0.01, hWB);
                if(i == 1)
                    [out, auxOut] = obj.step(specVelInc(i, :)', angleInc(i, :)', g(i), hAltm(i), ...
                        CNEExt(:, :, i), hExt(i), vNExt(i, :)', CBLExt(:, :, i), qBLExt(i, :), ...
                        beta_m_ext(i, :)', DvScul_m_ext(i, :)', DrScrl_m_ext(i, :)');
                    out = repmat(out, m+1, 1);
                    auxOut = repmat(auxOut, m+1, 1);
                else
                    [out(i), auxOut(i)] = obj.step(specVelInc(i, :)', angleInc(i, :)', g(i), hAltm(i), ...
                        CNEExt(:, :, i), hExt(i), vNExt(i, :)', CBLExt(:, :, i), qBLExt(i, :) , ...
                        beta_m_ext(i, :)', DvScul_m_ext(i, :)', DrScrl_m_ext(i, :)');
                end
            end
            [out(m+1), auxOut(m+1)] = obj.output(); % NOTE: step方法为先输出再更新，需要补充输出最后一次更新后的结果
            close(hWB);
            out = structarray2structofarray(out);
            auxOut = structarray2structofarray(auxOut);
        end
    end
    
    methods(Access = protected)
        %% Common functions
        function setupImpl(obj)
            % Perform one-time calculations, such as computing constants
            obj.EarthCst = struct('RE', obj.Earth_RE, 'f', obj.Earth_f, 'mu', obj.Earth_mu, ...
                'J2', obj.Earth_J2, 'J3', obj.Earth_J3, 'omegaIE', obj.Earth_omegaIE);
        end
        
        function resetImpl(obj)
            % Initialize / reset discrete-state properties
            % 零时刻初始化
            obj.DS_cycCnt = 0;
            obj.DS_n =0;
            obj.DS_m = 0;
            obj.DS_l = 0;
            obj.DS_Dalpha_lx = horzcat(obj.InitCon_angleInc_lx, zeros(3, 1));
            obj.DS_Dupsilon_lx = horzcat(obj.InitCon_specVelInc_lx, zeros(3, 1));
            obj.DS_supsilon_l = NaN(3, 1);
            obj.DS_salpha_l = NaN(3, 1);
            if obj.Cfg_UseDCMInAttCalc
                obj.DS_CB_mL_n = obj.InitCon_CBL;
                obj.DS_qB_mL_n = NaN(1, 4);
            else
                obj.DS_CB_mL_n = NaN(3);
                obj.DS_qB_mL_n = dcm2quat_cg(obj.InitCon_CBL'); % dcm2quat()将CLB转换为qBL
            end
            obj.DS_CB_mL_n1 = NaN(3);
            obj.DS_qB_mL_n1 = NaN(1, 4);
            obj.DS_CB_m1L_n1 = NaN(3);
            obj.DS_qB_m1L_n1 = NaN(1, 4);
            obj.DS_CL_n1L_m = NaN(3);
            obj.DS_CL_n1L_m1 = NaN(3);
            obj.DS_vN_m = obj.InitCon_vN_m;
            obj.DS_vN_m1 = obj.InitCon_vN_m1;
            obj.DS_vN_m2 = NaN(3, 1);
            obj.DS_CNE_n= dcmenu2ecef(obj.InitCon_LLA_n(1, 1), obj.InitCon_LLA_n(2, 1));
            obj.DS_CNE_n1 = NaN(3);
            obj.DS_h_n = obj.InitCon_LLA_n(3, 1);
            obj.DS_h_n1 = NaN;
            obj.DS_sumDRN_m = NaN(3, 1);
            obj.DS_sumDRN_m1 = NaN(3, 1);
            obj.DS_eVC3_n = 0;
            obj.DS_eVC3_n1 = NaN;
            obj.DS_omegaIEN = NaN(3, 1);
            obj.DS_g = obj.InitCon_g;
            obj.DS_beta_l = NaN(3, 1);
            obj.DS_DvScul_l = NaN(3, 1);
            obj.DS_DrScrl_l = NaN(3, 1);
            obj.DS_delh_n = NaN;
            [obj.DS_FCN_n, obj.DS_rhoNz_n, obj.DS_gN_n, obj.DS_omegaIEN_n] = obj.calcEarthPar();
            [obj.DS_FCN_n1, obj.DS_rhoNz_n1, obj.DS_gN_n1, obj.DS_omegaIEN_n1] = obj.calcEarthPar(true);
            [obj.DS_FCN_n2, obj.DS_rhoNz_n2, obj.DS_gN_n2, obj.DS_omegaIEN_n2] = deal(NaN(3), NaN, NaN(3, 1), NaN(3, 1));
        end
        
        function validatePropertiesImpl(obj)
            % Validate related or interdependent property values
            if any(abs(obj.InitCon_LLA_n(1:2, :)) > [1.5708; 3.1416]) || any(abs(obj.InitCon_LLA_n1(1:2, :)) > [1.5708; 3.1416])
                error('initial latitude must be in the range of [-pi/2, pi/2] and initial longitude must be in the range of [-pi, pi]');
            end
        end
        
        %% Simulink functions
        function ds = getDiscreteStateImpl(obj)
            % Return structure of properties with DiscreteState attribute
            ds = struct([]);
        end
        
        function flag = isInputSizeMutableImpl(obj, index)
            % Return false if input size cannot change
            % between calls to the System object
            flag = false;
        end
        
        function [out, out2] = getOutputSizeImpl(obj)
            % Return size for each output port
            out = [1, 1];
            out2 = [1, 1];
            
            % Example: inherit size from first input port
            % out = propagatedInputSize(obj, 1);
        end
        
        function [out, out2] = getOutputDataTypeImpl(obj)
            % Return data type for each output port
            out = obj.Cst_OutBusName;
            out2 = obj.Cst_AuxOutBusName;
            
            % Example: inherit data type from first input port
            % out = propagatedInputDataType(obj, 1);
        end
        
        function [out, out2] = isOutputComplexImpl(obj)
            % Return true for each output port with complex data
            out = false;
            out2 = false;
            
            % Example: inherit complexity from first input port
            % out = propagatedInputComplexity(obj, 1);
        end
        
        function [out, out2] = isOutputFixedSizeImpl(obj)
            % Return true for each output port with fixed size
            out = true;
            out2 = true;
            
            % Example: inherit fixed-size status from first input port
            % out = propagatedInputFixedSize(obj, 1);
        end
        
        function sts = getSampleTimeImpl(obj)
            % Define sample time type and parameters
            %    sts = obj.createSampleTime("Type", "Inherited");
            sts = obj.createSampleTime("Type", "Discrete", "SampleTime", obj.Par_Tl); % 偏移时间设置为0
            
            % Example: specify discrete sample time
            % sts = obj.createSampleTime("Type", "Discrete", ...
            %  "SampleTime", 1);
        end
        
        function updateImpl(obj, specVelInc, angleInc, g, hAltm, CNEExt, hExt, ...
                vNExt, CBLExt, qBLExt, beta_m_ext, DvScul_m_ext, DrScrl_m_ext)
            % Update discrete states as a function of input u
            if nargin < 13
                DrScrl_m_ext = NaN(3, 1);
            end
            if nargin < 12
                DvScul_m_ext = NaN(3, 1);
            end
            if nargin < 11
                beta_m_ext = NaN(3, 1);
            end
            if nargin < 10
                qBLExt = NaN(1, 4);
            end
            if nargin < 9
                CBLExt = NaN(3, 3, 1);
            end
            if nargin < 8
                vNExt = NaN(3, 1);
            end
            if nargin < 7
                hExt = NaN(1);
            end
            if nargin < 6
                CNEExt = NaN(3, 3, 1);
            end
            if nargin < 5
                hAltm = NaN(1);
            end
            if (nargin<4) || isempty(g)
                g = obj.InitCon_g;
            end
            
            % 导航解算周期
            obj.DS_delh_n = NaN;
            obj.DS_g = g;
            obj.DS_cycCnt = obj.DS_cycCnt + 1;
            % - - - - - - - - - - - - - - - 低速解算开始- - - - - - - - - - - - - - -
            if mod(obj.DS_cycCnt-1, obj.Cfg_NSInLS*obj.Cfg_HSInNS) == 0
                obj.DS_n = obj.DS_n + 1;
                % 更新低速解算历史量
                obj.DS_CNE_n1 = obj.DS_CNE_n;
                obj.DS_h_n1 = obj.DS_h_n;
                obj.DS_FCN_n2 = obj.DS_FCN_n1;
                obj.DS_FCN_n1 = obj.DS_FCN_n;
                obj.DS_rhoNz_n2 = obj.DS_rhoNz_n1;
                obj.DS_rhoNz_n1 = obj.DS_rhoNz_n;
                obj.DS_gN_n2 = obj.DS_gN_n1;
                obj.DS_gN_n1 = obj.DS_gN_n;
                obj.DS_omegaIEN_n2 = obj.DS_omegaIEN_n1;
                obj.DS_omegaIEN_n1 = obj.DS_omegaIEN_n;
                obj.DS_eVC3_n1 = obj.DS_eVC3_n;
                % 中速解算零时刻初始化
                obj.DS_m = 0;
                if obj.Cfg_UseDCMInAttCalc
                    obj.DS_CB_mL_n1 = obj.DS_CB_mL_n;
                else
                    obj.DS_qB_mL_n1 = obj.DS_qB_mL_n;
                end
                obj.DS_CL_n1L_m = eye(3);
                obj.DS_sumDRN_m = zeros(3, 1);
            end
            % ------------------------------中速解算开始------------------------------
            if mod(obj.DS_cycCnt-1, obj.Cfg_HSInNS) == 0
                obj.DS_m = obj.DS_m + 1;
                % 更新中速解算历史量
                if obj.Cfg_UseDCMInAttCalc
                    obj.DS_CB_m1L_n1 = obj.DS_CB_mL_n1;
                else
                    obj.DS_qB_m1L_n1 = obj.DS_qB_mL_n1;
                    obj.DS_CB_m1L_n1 = quat2dcm_cg(obj.DS_qB_m1L_n1)'; % quat2dcm()将qBL转换为CLB
                end
                obj.DS_CL_n1L_m1 = obj.DS_CL_n1L_m;
                obj.DS_sumDRN_m1 = obj.DS_sumDRN_m;
                obj.DS_vN_m2 = obj.DS_vN_m1;
                obj.DS_vN_m1 = obj.DS_vN_m;
                % 高速解算零时刻初始化
                obj.DS_l = 0;
                obj.BCAU.reset();
                [~, mid_alpha_l1, ~] = obj.BCAU.output(obj.DS_Dalpha_lx); % 调用step或output之前，不能调用update方法
                obj.BCSVU.reset();
                obj.BCSVU.output(obj.DS_Dupsilon_lx, mid_alpha_l1, obj.DS_Dalpha_lx);
                obj.DS_DrScrl_l = zeros(3, 1);
                obj.DS_supsilon_l = zeros(3, 1);
                obj.DS_salpha_l = zeros(3, 1);
            end
            % ==============================高速解算开始==============================
            obj.DS_l = obj.DS_l + 1;
            % 更新高速解算历史量，读入器件输出
            obj.DS_Dupsilon_lx = horzcat(specVelInc, obj.DS_Dupsilon_lx(:, 1:(end-1)));
            obj.DS_Dalpha_lx = horzcat(angleInc, obj.DS_Dalpha_lx(:, 1:(end-1)));
            mid_DrScrl_l1 = obj.DS_DrScrl_l;
            mid_supsilon_l1 = obj.DS_supsilon_l;
            mid_salpha_l1 = obj.DS_salpha_l;
            % 运行高速解算步骤
            obj.BCAU.update(obj.DS_Dalpha_lx);
            [mid_alpha_l, mid_alpha_l1, obj.DS_beta_l] = obj.BCAU.output(obj.DS_Dalpha_lx);
            obj.BCSVU.update(obj.DS_Dupsilon_lx, mid_alpha_l1, obj.DS_Dalpha_lx);
            [mid_upsilon_l, mid_upsilon_l1, obj.DS_DvScul_l, mid_DvScul_l1] = obj.BCSVU.output(obj.DS_Dupsilon_lx, mid_alpha_l1, obj.DS_Dalpha_lx);
            [obj.DS_DrScrl_l, obj.DS_supsilon_l, obj.DS_salpha_l] = pos_hs(mid_DrScrl_l1, mid_supsilon_l1, mid_salpha_l1, mid_DvScul_l1, mid_upsilon_l1, obj.DS_Dupsilon_lx, ...
                mid_alpha_l1, obj.DS_Dalpha_lx, obj.Par_Tl, @drScrl_l_2ndod, @Dsupsilon_l_2ndod, @Dsalpha_l_2ndod);
            % ==============================高速解算结束==============================
            if mod(obj.DS_cycCnt, obj.Cfg_HSInNS) == 0
                % 运行中速解算步骤
                [mid_DvSFB_m1_m] = obj.BCSVU.step_ns(mid_alpha_l, DvScul_m_ext);
                earthPar_n1 = struct('FCN', obj.DS_FCN_n1, 'rhoNz', obj.DS_rhoNz_n1, 'gN', obj.DS_gN_n1, 'omegaIEN', obj.DS_omegaIEN_n1);
                earthPar_n2 = struct('FCN', obj.DS_FCN_n2, 'rhoNz', obj.DS_rhoNz_n2, 'gN', obj.DS_gN_n2, 'omegaIEN', obj.DS_omegaIEN_n2);
                [obj.DS_vN_m, mid_DvSFLI_n1_m, mid_DvGCorN_m, obj.DS_CL_n1L_m] = vel_ns(obj.DS_vN_m1, obj.DS_vN_m2, obj.DS_sumDRN_m1, mid_DvSFB_m1_m, ...
                    obj.DS_CB_m1L_n1, obj.DS_CL_n1L_m1, earthPar_n1, earthPar_n2, obj.Cfg_NSInLS, ...
                    obj.DS_m, obj.Par_Tl*obj.Cfg_HSInNS, obj.Cst);
                if ~any(isnan(vNExt))
                    obj.DS_vN_m = vNExt;
                end
                if obj.Cfg_UseHiResPosNSCal
                    if isempty(DrScrl_m_ext) || any(isnan(DrScrl_m_ext))
                        mid_DrScrl_m = obj.DS_DrScrl_l;
                    else
                        mid_DrScrl_m = DrScrl_m_ext;
                    end
                    [~, obj.DS_sumDRN_m] = pos_ns_hires(obj.DS_sumDRN_m1, mid_DrScrl_m, obj.DS_supsilon_l, obj.DS_salpha_l, ...
                        mid_upsilon_l, mid_alpha_l, obj.DS_vN_m1, mid_DvSFLI_n1_m, mid_DvGCorN_m, obj.DS_CL_n1L_m, obj.DS_CL_n1L_m1, ...
                        obj.DS_CB_m1L_n1, obj.Par_Tl*obj.Cfg_HSInNS, obj.Cst, @DrRot_m_exactform_5thod);
                else
                    [~, obj.DS_sumDRN_m] = pos_ns_trapez(obj.DS_sumDRN_m1, obj.DS_vN_m, obj.DS_vN_m1, obj.Par_Tl*obj.Cfg_HSInNS);
                end
                [mid_CB_mB_m1, mid_qB_mB_m1] = obj.BCAU.step_ns(obj.Cfg_UseDCMInAttCalc, beta_m_ext);
                if obj.Cfg_UseDCMInAttCalc
                    obj.DS_CB_mL_n1 = obj.DS_CB_m1L_n1 * mid_CB_mB_m1;
                else
                    obj.DS_qB_mL_n1 = quatmultiply(obj.DS_qB_m1L_n1, mid_qB_mB_m1);
                end
            end
            % ------------------------------中速解算结束------------------------------
            if mod(obj.DS_cycCnt, obj.Cfg_HSInNS*obj.Cfg_NSInLS) == 0
                % 运行低速解算步骤
                if any(any(isnan(CNEExt))) || isnan(hExt)
                    earthPar_n1 = struct('FCN', obj.DS_FCN_n1, 'rhoNz', obj.DS_rhoNz_n1);
                    earthPar_n2 = struct('FCN', obj.DS_FCN_n2, 'rhoNz', obj.DS_rhoNz_n2);
                    [obj.DS_CNE_n, obj.DS_h_n, obj.DS_delh_n] = pos_ls(obj.DS_CNE_n1, obj.DS_h_n1, obj.DS_sumDRN_m, ...
                        earthPar_n1, earthPar_n2, obj.Par_Tl*obj.Cfg_HSInNS*obj.Cfg_NSInLS, ...
                        hAltm, obj.Par_C, obj.Cst, @rv2dcm_5thod);
                    if ~any(any(isnan(CNEExt)))
                        obj.DS_CNE_n = CNEExt;
                    end
                    if ~isnan(hExt)
                        obj.DS_h_n = hExt;
                    end
                else
                    obj.DS_CNE_n = CNEExt;
                    obj.DS_h_n = hExt;
                    obj.DS_delh_n = hExt - hAltm;
                end
                if any(isnan(vNExt)) && any(obj.Par_C) % 考虑阻尼的高度通道修正
                    [obj.DS_vN_m, obj.DS_eVC3_n] = vel_ls(obj.DS_vN_m, obj.Par_Tl*obj.Cfg_HSInNS*obj.Cfg_NSInLS, obj.DS_delh_n, obj.DS_eVC3_n1, obj.Par_C, obj.Cst);
                end
                [obj.DS_FCN_n, obj.DS_rhoNz_n, obj.DS_gN_n, obj.DS_omegaIEN_n] = obj.calcEarthPar();
                earthPar_n = struct('FCN', obj.DS_FCN_n, 'rhoNz', obj.DS_rhoNz_n, 'omegaIEN', obj.DS_omegaIEN_n);
                earthPar_n1 = struct('FCN', obj.DS_FCN_n1, 'rhoNz', obj.DS_rhoNz_n1, 'omegaIEN', obj.DS_omegaIEN_n1);
                if obj.Cfg_UseDCMInAttCalc
                    if any(any(isnan(CBLExt)))
                        [obj.DS_CB_mL_n] = att_ls_dcm(obj.DS_CB_mL_n1, obj.DS_sumDRN_m, earthPar_n, earthPar_n1, ...
                            obj.Par_Tl*obj.Cfg_HSInNS*obj.Cfg_NSInLS, obj.Cst, obj.Cfg_UseNormOrthoInAttCalc, @rv2dcm_5thod);
                    else
                        obj.DS_CB_mL_n = CBLExt;
                    end
                else
                    if any(isnan(qBLExt), 2)
                        [obj.DS_qB_mL_n] = att_ls_quat(obj.DS_qB_mL_n1, obj.DS_sumDRN_m, earthPar_n, earthPar_n1, ...
                            obj.Par_Tl*obj.Cfg_HSInNS*obj.Cfg_NSInLS, obj.Cst, obj.Cfg_UseNormOrthoInAttCalc, @rv2quat_5thod);
                    else
                        obj.DS_qB_mL_n = qBLExt;
                    end
                end
            end
            % - - - - - - - - - - - - - - - 低速解算结束- - - - - - - - - - - - - - -
        end
        
        function [out, auxOut] = outputImpl(obj, specVelInc, angleInc, g, hAltm, CNEExt, hExt, ...
                vNExt, CBLExt, qBLExt, beta_m_ext, DvScul_m_ext, DrScrl_m_ext)
            % Calculate output y as a function of discrete states and
            % direct feedthrough inputs
            out.cycCnt = obj.DS_cycCnt;
            out.LSCnt = obj.DS_n;
            out.NSCnt = obj.DS_m;
            out.HSCnt = obj.DS_l;
            if obj.Cfg_UseDCMInAttCalc
                tmp_CBL = obj.DS_CB_mL_n; % CBI_mLI_n在低速解算时更新
            else
                tmp_CBL = quat2dcm_cg(obj.DS_qB_mL_n)'; % quat2dcm()将qBL转换为CLB
            end
            [tmp_L, tmp_lambda, tmp_vG, tmp_CBN, tmp_CBG, tmp_phi, tmp_theta, tmp_psi, tmp_psiTrue, tmp_alpha] = wholenavparam(obj.DS_CNE_n, obj.DS_vN_m', tmp_CBL);
            out.CNE = obj.DS_CNE_n; % CNE_n在低速解算时更新
            out.L = tmp_L;
            out.lambda = tmp_lambda;
            out.h = obj.DS_h_n; % h_n在低速解算时更新
            out.vN = obj.DS_vN_m; % vN_m在中速解算时更新
            out.vG = tmp_vG';
            out.CBL = tmp_CBL;
            out.qBL = obj.DS_qB_mL_n;
            out.CBN = tmp_CBN;
            out.CBG = tmp_CBG;
            out.phi = tmp_phi;
            out.theta = tmp_theta;
            out.psi = tmp_psi;
            out.psiTrue = tmp_psiTrue;
            out.alpha = tmp_alpha;
            out.gN = obj.DS_gN_n;
            out.FCN = obj.DS_FCN_n;
            out.omegaIEN = obj.DS_omegaIEN_n;
            out.omegaENN = obj.DS_rhoNz_n*obj.Cst_uZNN + obj.DS_FCN_n*cross(obj.Cst_uZNN, obj.DS_vN_m);
            
            auxOut.delh_n = obj.DS_delh_n;
            auxOut.eVC3_n = obj.DS_eVC3_n;
            auxOut.beta_l = obj.DS_beta_l;
            auxOut.DvScul_l = obj.DS_DvScul_l;
            auxOut.DrScrl_l = obj.DS_DrScrl_l;
        end
        
        function [flag, flag2, flag3, flag4, flag5, flag6, flag7, flag8, flag9, flag10, flag11, flag12, flag13] = isInputDirectFeedthroughImpl(obj, specVelInc, angleInc, g, hAltm, CNEExt, hExt, vNExt, CBLExt, qBLExt, beta_m_ext, DvScul_m_ext, DrScrl_m_ext)
            % Return true if input u is needed to calculate the output at
            % the same time
            flag = false;
            flag2 = false;
            flag3 = false;
            flag4 = false;
            flag5 = false;
            flag6 = false;
            flag7 = false;
            flag8 = false;
            flag9 = false;
            flag10 = false;
            flag11 = false;
            flag12 = false;
            flag13 = false;
        end
        
        %% Customized functions
        function [FCN, rhoNz, gN, omegaIEN] = calcEarthPar(obj, useInitConPrevStepData)
            if nargin < 3
                useInitConPrevStepData = false;
            end
            if useInitConPrevStepData
                earthPar = calcearthpar_pgs(dcmenu2ecef(obj.InitCon_LLA_n1(1, 1), obj.InitCon_LLA_n1(2, 1)), ...
                    obj.InitCon_h_n1, obj.InitCon_vN_n1(1, 1), obj.Cfg_NavCoordType, obj.Cfg_UseExtGravity, obj.InitCon_g, obj.EarthCst);
            else
                earthPar = calcearthpar_pgs(obj.DS_CNE_n, obj.DS_h_n, obj.DS_vN_m(1, 1), ...
                    obj.Cfg_NavCoordType, obj.Cfg_UseExtGravity, obj.DS_g, obj.EarthCst);
            end
            FCN = earthPar.FCN;
            rhoNz = earthPar.rhoNz;
            gN = earthPar.gN;
            omegaIEN = earthPar.omegaIEN;
        end
    end
end